/**
 * @file SolverMWAR.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/09/19
//  a general solver for MW filtering 
//
//  2019/10/05
//  use inverseDiag to replace the inverseChol
//
//  2020/02/17
//  seperate SolverMW/SolverMWAR for easy-using
//
//============================================================================


#include "SolverMWAR.hpp"
#include "SystemTime.hpp"
#include "Equation.hpp"
#include "Variable.hpp"
#include "AmbDatum.hpp"

#define debug 0
#define detail 0

using namespace gpstk::StringUtils;

namespace gpstk
{

      // Index initially assigned to this class
   int SolverMWAR::classIndex = 9600001;


      // Returns an index identifying this object.
   int SolverMWAR::getIndex() const
   { return index; }


      // Returns a string identifying this object.
   std::string SolverMWAR::getClassName() const
   { return "SolverMWAR"; }

   void SolverMWAR::Init(void)
   {}

   void SolverMWAR::setUpEquations(void)
   {

       ///////////////////////////////////////////////
       //
       // Now, define variables need to be estimated 
       // 
       ///////////////////////////////////////////////
       
       // The following variables are, SourceID and SatID-indexed
       Variable BWL12GPS( TypeID::BWL12, &BWL12GPSModel, true, true, true );
       Variable BWL15GPS( TypeID::BWL15, &BWL15GPSModel, true, true, true );

       Variable BWL15GAL( TypeID::BWL15, &BWL15GALModel, true, true, true );
       Variable BWL17GAL( TypeID::BWL17, &BWL17GALModel, true, true, true );
       Variable BWL18GAL( TypeID::BWL18, &BWL18GALModel, true, true, true );
       Variable BWL16GAL( TypeID::BWL16, &BWL16GALModel, true, true, true );

       Variable BWL27BDS( TypeID::BWL27, &BWL27BDSModel, true, true, true );
       Variable BWL26BDS( TypeID::BWL26, &BWL26BDSModel, true, true, true );

       Variable bias12GPS(TypeID::biasWL12, &constBiasModel);
       Variable bias15GPS(TypeID::biasWL15, &constBiasModel);

       Variable bias15GAL(TypeID::biasWL15, &constBiasModel);
       Variable bias17GAL(TypeID::biasWL17, &constBiasModel);
       Variable bias18GAL(TypeID::biasWL15, &constBiasModel);
       Variable bias16GAL(TypeID::biasWL16, &constBiasModel);

       Variable bias27BDS(TypeID::biasWL27, &constBiasModel);
       Variable bias26BDS(TypeID::biasWL26, &constBiasModel);

       //
       // Define Equation object for code/phase observations, and add variables 
       //

       // GPS PC
       Equation equMW12GPS(TypeID::MW12);
       Equation equMW15GPS(TypeID::MW15);

       Equation equMW15GAL(TypeID::MW15, SatelliteSystem::Galileo);
       Equation equMW17GAL(TypeID::MW17, SatelliteSystem::Galileo);
       Equation equMW18GAL(TypeID::MW18, SatelliteSystem::Galileo);
       Equation equMW16GAL(TypeID::MW16, SatelliteSystem::Galileo);

       Equation equMW27BDS(TypeID::MW27, SatelliteSystem::BDS);
       Equation equMW26BDS(TypeID::MW26, SatelliteSystem::BDS);

       //////////////////////////
       // insert variables now
       //////////////////////////
       // GPS
       equMW12GPS.addVariable(BWL12GPS, WL12_WAVELENGTH_GPS);
       equMW15GPS.addVariable(BWL15GPS, WL15_WAVELENGTH_GPS);

       equMW15GAL.addVariable(BWL15GAL, WL15_WAVELENGTH_GAL);
       equMW17GAL.addVariable(BWL17GAL, WL17_WAVELENGTH_GAL);
       equMW18GAL.addVariable(BWL18GAL, WL18_WAVELENGTH_GAL);
       equMW16GAL.addVariable(BWL16GAL, WL16_WAVELENGTH_GAL);

       equMW27BDS.addVariable(BWL27BDS, WL27_WAVELENGTH_BDS);
       equMW26BDS.addVariable(BWL26BDS, WL26_WAVELENGTH_BDS);

       //////////////////////
       // 
       // MW = bw + bs + Nw; 
       // 
       //////////////////////
       equMW12GPS.addVariable(bias12GPS, 1.0);
       equMW15GPS.addVariable(bias15GPS, 1.0);

       equMW15GAL.addVariable(bias15GAL, 1.0);
       equMW17GAL.addVariable(bias17GAL, 1.0);
       equMW18GAL.addVariable(bias18GAL, 1.0);
       equMW16GAL.addVariable(bias16GAL, 1.0);

       equMW27BDS.addVariable(bias27BDS, 1.0);
       equMW26BDS.addVariable(bias26BDS, 1.0);

       // define the weight for all combinations relative PC12GPS

       // Phase equations should have higher relative weight
       // var(BWL) ~= 1/2*0.3^2 
       // weight ~= 2/0.3^2 = 22
       equMW12GPS.setWeight(22.0);     // 100.0 * 100.0
       equMW15GPS.setWeight(22.0);     // 100.0 * 100.0

       equMW15GAL.setWeight(22.0);
       equMW17GAL.setWeight(22.0);
       equMW18GAL.setWeight(22.0);
       equMW16GAL.setWeight(22.0);

       equMW27BDS.setWeight(22.0);
       equMW26BDS.setWeight(22.0);

          ///////////
          // Now, the real-equation that are actived in the equationSystems.
          ///////////
       
       // YOU MUST clear the equation system firstly!!!!!
       // only current equations is feed to equSystem
       equSystem.clearEquations();
       
          // Add equation descriptions
       if(satSys == SatelliteSystem::GPS)
       {
           if( mwType == TypeID::MW12 )
           {
               equSystem.addEquation(equMW12GPS);
           }
           else if ( mwType == TypeID::MW15 )
           {
               equSystem.addEquation(equMW15GPS);
           }
       }
       else if(satSys == SatelliteSystem::Galileo)
       {
           if( mwType == TypeID::MW15)
           {
               equSystem.addEquation(equMW15GAL);
           }
           else if(mwType == TypeID::MW17)
           {
               equSystem.addEquation(equMW17GAL);
           }
           else if(mwType == TypeID::MW18)
           {
               equSystem.addEquation(equMW18GAL);
           }
           else if(mwType == TypeID::MW16)
           {
               equSystem.addEquation(equMW16GAL);
           }
       }
       else if(satSys == SatelliteSystem::BDS)
       {
           if( mwType == TypeID::MW27 )
           {
               equSystem.addEquation(equMW27BDS);
           }
           else if ( mwType == TypeID::MW26 )
           {
               equSystem.addEquation(equMW26BDS);
           }
       }
    

   } // end of setUpEquations



      /* Returns a reference to a gnssRinex object after solving
       *  the previously defined equation system.
       *
       * @param gData    Data object holding the data.
       */
   void SolverMWAR::Process(commonHeader& comHeader, gnssRinex& gData)
       noexcept(false)
   {

      try
      {
         if(gData.numSats() == 0)
         {
             return;
         }

         preCompute( gData );
         Compute( gData );
         postCompute( gData );
      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );
         GPSTK_THROW(e);
      }

      return;

   }  // End of method 'SolverMWAR::Prepare()'



      /* Code to be executed before 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverMWAR::preCompute( gnssRinex& gRin)
       noexcept(false)
   {
      try
      {

         if(debug)
         {
             CivilTime cv(gRin.header.epoch) ;
             cout << "start of SolverMWAR at epoch:" << cv << endl;
         }


            // Prepare the equation system with current data
         equSystem.Prepare(gRin);

            // Get the number of unknowns being processed
         int numUnknowns( equSystem.getTotalNumVariables() );

            // Get the set with unknowns being processed
         currentUnkSet = ( equSystem.getVarUnknowns() );

         if(debug)
         {
             cout << "currentUnkSet are as follows:" << endl;
             for(VariableSet::iterator vIt=currentUnkSet.begin();
                     vIt!=currentUnkSet.end();
                     vIt++)
             {
                 cout << (*vIt) << endl;
             }
         }


            // Get matrices and vectors out of equation system
            // Measurements vector (Prefit-residuals)
         measVector = equSystem.getPrefitsVector();
         hMatrix = equSystem.getGeometryMatrix();
         rMatrix = equSystem.getWeightsMatrix();

         if(debug)
         {
             cout << "measVector:" << endl;
             cout << measVector << endl;
             cout << hMatrix << endl;
             cout << rMatrix << endl;
         }

            // State Transition Matrix (PhiMatrix)
         phiMatrix = equSystem.getPhiMatrix();
         qMatrix = equSystem.getQMatrix();

         if(debug)
         {
             YDSTime yds(gRin.header.epoch);

             cout << yds.year << " " 
                  << yds.doy  << " "
                  << yds.sod  << " " 
                  << endl;

             cout << "phiMatrix" << endl;
             cout << phiMatrix << endl;
             cout << qMatrix << endl;

             cout << "measVector" << endl;
             cout << measVector << endl;
             cout << hMatrix<< endl;
             cout << rMatrix<< endl;
         }

            // Feed the filter with the correct state and covariance matrix
         if(firstTime)
         {

            Vector<double> initialState(numUnknowns, 0.0);
            Matrix<double> initialCov( numUnknowns, numUnknowns, 0.0 );

               // Fill the initialCov matrix
            for( auto v: currentUnkSet )
            {
                int index = v.getNowIndex();
                initialCov(index,index) = v.getInitialVariance();
            }


               // Reset Kalman filter state and covariance matrix
            xhat = initialState; 
            P    = initialCov;

            // first epoch
            firstEpoch = gRin.header.epoch;

               // No longer first time
            firstTime = false;
         }
         else
         {

               // Adapt the size to the current number of unknowns
            Vector<double> currentState(numUnknowns, 0.0);
            Matrix<double> currentCov(numUnknowns, numUnknowns, 0.0);


               // Fill the state vector
            for( auto v: currentUnkSet )
            {
                int now_index = v.getNowIndex();
                int old_index = v.getPreIndex();
                if( old_index != -1)
                {
                    currentState(now_index) = solution( old_index );
                }
                else
                {
                    currentState(now_index) = 0.0;
                }
            }

               // Fill the covariance matrix, 2019/08/01, shjzhang

               // We need a copy of 'currentUnkSet'
            VariableSet tempSet( currentUnkSet );
            for( auto v: currentUnkSet )
            {
                int nowIndex = v.getNowIndex();
                int oldIndex = v.getPreIndex();
                if( oldIndex != -1 )
                {
                    for( auto v2: tempSet )
                    {
                        int nowIndex2 = v2.getNowIndex();
                        int oldIndex2 = v2.getPreIndex();

                        if( oldIndex2 != -1 )
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = covMatrix(oldIndex, oldIndex2);
                        }
                        else
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = 0.0;
                        }
                    }
                }
                else
                {
                    currentCov(nowIndex, nowIndex) = v.getInitialVariance();
                }
                // erase var
                tempSet.erase(v);
            }  // End of for( VariableSet::const_iterator itVar1 = currentUnkSet...'


               // Reset Kalman filter to current state and covariance matrix
            xhat = currentState;
            P    = currentCov;

         }  // End of 'if(firstTime)'

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverMWAR::preCompute()'



      // Compute the solution of the given equations set.
      //
      // @param gData    Data object holding the data.
      //
      // \warning A typical kalman filter will be devided into 
      // two different phase, the prediction and the correction.
      // for some special application, for example, the 'integer'
      // satellite and receiver clock/bias estimation, the ambiguity
      // constraints are generated using the 'predicted' ambiguity 
      // values and its variances. 
      //
   gnssRinex& SolverMWAR::Compute( gnssRinex& gRin )
       noexcept(false)
   {
      /////////////////////////////////////
      //
      // TimeUpdate( phiMatrix, qMatrix );
      //
      /////////////////////////////////////

      Matrix<double> phiT(transpose(phiMatrix));

         // Compute the a priori state vector
      xhatminus = phiMatrix*xhat;
         // Compute the a priori estimate error covariance matrix
      Pminus = phiMatrix*P*phiT + qMatrix;

        
      ////////////////////////////////////////
      //
      // Ambiguity constraint
      //
      // MW = br + Bw;
      // 
      // one ambiguity must be fixed to remove the rank deficient
      //
      ////////////////////////////////////////

      if(detail)
      {
        cout << "xhatminus" << endl;
        cout <<  xhatminus  << endl;

        cout << "Pminus" << endl;
        cout <<  Pminus  << endl;

        cout << "measVector"<< endl;
        cout << measVector << endl;
        cout << "hMatrix" << endl;
        cout << hMatrix << endl;
        cout << "rMatrix" << endl;
        cout << rMatrix << endl;
      }

      // get ambs 
      VariableDataMap ambData;    
      VariableDataMap ambVarData;
      for(auto v: currentUnkSet)
      {
          if(v.getArcIndexed())
          {
              ambData[v] = xhatminus(v.getNowIndex());
              ambVarData[v] = Pminus(v.getNowIndex(), v.getNowIndex());
          }
      }

      if(debug)
      {
          cout << "ambData WL" << endl;
          for(auto amb: ambData)
          {
              cout << amb.first << ":" << amb.second << " var:" << ambVarData[amb.first] << endl;
          }
      }

      // select datum amb
      AmbDatum selectDatum;
      VariableDataMap datumData = selectDatum.Prepare(ambData, ambVarData);

      if(debug)
      {
          cout << "datumData for WL" << endl;
          for(auto dd: datumData)
          {
              cout << dd.first << ":" << dd.second << endl;
          }
      }

      if(datumData.size()==0)
      {
          cerr << "datumData is not found" << endl;
          exit(-1);
      }

      auto datumIter = datumData.begin();
      if(ambData.find( datumIter->first ) == ambData.end())
      {
          cerr << "SolverMWAR:: datumData is not found!"<< endl;
          exit(-1);
      }

      // now, augment the measVector
      int numEqu = measVector.size();
      int numUnknowns = currentUnkSet.size();

      int numEquALL = ( numEqu + 1 ) ;

      Vector<double> tempPrefit(numEquALL, 0.0);
      Matrix<double> tempGeometry(numEquALL, numUnknowns, 0.0);
      Matrix<double> tempWeight(numEquALL, numEquALL, 0.0);

      for(int i=0; i<numEqu; i++)
      {
            // Prefit
         tempPrefit(i) = measVector(i);
            // Geometry
         for(int j=0; j<numUnknowns; j++)
         {
            tempGeometry(i,j) = hMatrix(i,j);
         }
            // diagnoal Weight
         tempWeight(i,i) = rMatrix(i,i);
      }

      // ambiguity datum equation
      tempPrefit(numEqu) = datumIter->second;
      // the right column for datum amb
      tempGeometry(numEqu, (*datumIter).first.getNowIndex()) = 1.0;
      tempWeight(numEqu, numEqu) = 1.0E+9;

      measVector.resize(numEquALL, 0.0);
      hMatrix.resize(numEquALL, numUnknowns, 0.0);
      rMatrix.resize(numEquALL, numEquALL, 0.0);

      measVector = tempPrefit;
      hMatrix = tempGeometry;
      rMatrix = tempWeight;

      if(detail)
      {
         cout << "constraint" << endl;
         cout << "measVector"<< endl;
         cout << measVector << endl;
         cout << "hMatrix" << endl;
         cout << hMatrix << endl;
         cout << "rMatrix" << endl;
         cout << rMatrix << endl;
      }

      /////////////////////////////////////
      //
      // MeasUpdate( measVector, hMatrix, rMatrix );
      //
      /////////////////////////////////////
        
         // After checking sizes, let's do the real correction work
      Matrix<double> invPMinus;
      Matrix<double> hMatrixT( transpose(hMatrix) );

      try
      {
         // shjzhang, see MatrixOperator 
         invPMinus = inverseChol(Pminus);

         Matrix<double> invTemp( hMatrixT * rMatrix * hMatrix +
                                 invPMinus );

            // Compute the a posteriori error covariance matrix
         P = inverseChol( invTemp );

            // Compute the a posteriori state estimation
         xhat = P * ( (hMatrixT * rMatrix * measVector) + 
                      (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return gRin;
      }

      xhatminus = xhat;
      Pminus = P;

      solution = xhat;
      covMatrix = P;
         // Compute the postfit residuals Vector
      postfitResiduals = measVector - (hMatrix * solution);

         // Return  
      return gRin;

   }  // End of method 'SolverMWAR::Compute()'


      /* Code to be executed after 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverMWAR::postCompute( gnssRinex& gRin )
       noexcept(false)
   {
      try
      {
         CommonTime epoch(gRin.header.epoch);
         gnssRinex gBack(gRin);

         TypeIDSet typeSet;
         typeSet.insert(TypeID::weight);
         typeSet.insert(TypeID::satArc);
         typeSet.insert(TypeID::elevation);

         typeSet.insert(TypeID::BWL12);
         typeSet.insert(TypeID::BWL15);
         typeSet.insert(TypeID::BWL17);
         typeSet.insert(TypeID::BWL18);
         typeSet.insert(TypeID::BWL16);
         typeSet.insert(TypeID::BWL27);
         typeSet.insert(TypeID::BWL26);

         typeSet.insert(TypeID::MW12);
         typeSet.insert(TypeID::MW15);
         typeSet.insert(TypeID::MW17);
         typeSet.insert(TypeID::MW18);
         typeSet.insert(TypeID::MW16);
         typeSet.insert(TypeID::MW27);
         typeSet.insert(TypeID::MW26);


         gBack.keepOnlyTypeID(typeSet);

         if(debug)
         {
             for(auto v: currentUnkSet)
             {
                 cout << v << endl;
             }

             cout << "solution" << endl;
             cout << solution << endl;
         }

         if(forwardProcessing)
         {

            // print float mw 
            if(pObsStream!=NULL)
                printObs( pObsStream, epoch, gRin);

            VariableEstDataMap ambEstData;
            ambEstData = getAmbs(solution, covMatrix);

            if(pAmbStream!=NULL)
                printAmbs(pAmbStream, epoch, ambEstData);

            // fix ambiguities for BWL
            VariableDataMap fixedVarData;
            partialAR.Round(ambEstData, fixedVarData);

            // print out ambiguity information
            if(pFixedAmbStream!=NULL)
               printFixedAmbs(pFixedAmbStream, epoch, ambEstData, fixedVarData);

            TypeID recType;
            double recBias;
            getRecBias(solution, recType, recBias);

            if(debug)
            {
                cout << "fixedVarData" << endl;
                for(auto vd: fixedVarData)
                {
                    cout << vd.first << ":" << vd.second << endl;
                }
            }

            int numAmb = ambEstData.size();
            int numFixed = fixedVarData.size();
            
            if(pFixedInfoStream!=NULL)
                printFixedInfo(pFixedInfoStream, epoch, recType, recBias, numAmb, numFixed);

            // convert BWL12 to BLC12
            VariableDataMap tempFixedAmbs;
            convertToBLCIndexed(fixedVarData, tempFixedAmbs);

            currentFixedAmbs = tempFixedAmbs;

            forwardRinexData[epoch] = gBack;
            forwardStateData[epoch] = solution;
            forwardCovData[epoch] = covMatrix;
         }

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverMWAR::postCompute()'

   void SolverMWAR::PostProcess()
       noexcept(false)
   {

     try
     {
         // now, backward processing
         forwardProcessing=false;

         // loop all saved data in forward processing
         globalAmbEstData.clear();
         globalFixedAmbs.clear();
         for(epochGnssRinexMap::reverse_iterator reverseIt = forwardRinexData.rbegin();
             reverseIt != forwardRinexData.rend();
             ++reverseIt)
         {
             CommonTime epoch( (*reverseIt).first );

             gnssRinex gRin( (*reverseIt).second );

             // Backward filtering
             Process(gRin);

             // Get forward state/cov from forwardStateData and forwardCovData
             Vector<double> lastStateVec;
             Matrix<double> lastCovMatrix;

             lastStateVec = forwardStateData[epoch];
             lastCovMatrix = forwardCovData[epoch];

             Vector<double> smoothedStateVec;
             Matrix<double> smoothedCovMatrix;

             // smoothe the forward and backward state and covariance
             smoothSolution(lastStateVec, lastCovMatrix, 
                            smoothedStateVec, smoothedCovMatrix);

             VariableEstDataMap ambEstData;
             ambEstData = getAmbs(smoothedStateVec, smoothedCovMatrix);
             for(auto ae: ambEstData)
             {
                globalAmbEstData[ae.first] = ae.second;
             }

             // print smoothed-float solutions
             if(pAmbStreamSmoothed!=NULL)
                 printAmbs(pAmbStreamSmoothed, epoch, ambEstData);

             // fix ambiguities for BWL
             VariableDataMap fixedVarData;
             partialAR.Round(ambEstData, fixedVarData);

             // convert BWL12 to BLC12
             VariableDataMap tempFixedAmbs;
             convertToBLCIndexed(fixedVarData, tempFixedAmbs);

             for(auto vd: tempFixedAmbs)
             {
                 globalFixedAmbs[vd.first] = vd.second; 
             }

             // print out ambiguity information
             if(pFixedAmbStreamSmoothed!=NULL)
                 printFixedAmbs(pFixedAmbStreamSmoothed, epoch, ambEstData, fixedVarData);

             TypeID recType;
             double recBias;
             getRecBias(smoothedStateVec, recType, recBias);

             int numAmb = ambEstData.size();
             int numFixed = fixedVarData.size();
             
             if(pFixedInfoStreamSmoothed!=NULL)
                 printFixedInfo(pFixedInfoStreamSmoothed, epoch, recType, recBias, 
                                numAmb, numFixed);

         } // end of 'for(epoch)'


         // at last free the random
         forwardRinexData.clear();
         forwardStateData.clear();
         forwardCovData.clear();

         int totalNum = globalAmbEstData.size();
         int totalFixedNum = globalFixedAmbs.size();
         cout << "totalFixedNum: " << totalFixedNum << "totalNum:" << totalNum << endl;

     }
     catch(Exception& u)
     {
           // Throw an exception if something unexpected happens
        ProcessingException e( getClassName() + ":"
                               + StringUtils::asString( getIndex() ) + ":"
                               + u.what() );

        GPSTK_THROW(e);

     }

       return;
   }

   void SolverMWAR::smoothSolution( Vector<double>& oldStateVec, 
                                   Matrix<double>& oldCovMatrix,
                                   Vector<double>& smoothedStateVec, 
                                   Matrix<double>& smoothedCovMatrix)
   {

       try
       {
           if(solution.size() != oldStateVec.size())
           {
              InvalidSolver e("smoothSolution(): size of stateMap don't match size of the oldState");
              GPSTK_THROW(e);
           }

           Matrix<double> currentWeight;
           Matrix<double> oldWeight;

           try
           {
               currentWeight = inverseChol(covMatrix);
               oldWeight = inverseChol(oldCovMatrix);
           }
           catch(Exception& u)
           {
               cerr << "SolverPPP::smoothSolution: covMatrix inversion error!" << endl;
               exit(-1);
           }

           smoothedCovMatrix = inverseChol( currentWeight +  oldWeight);
           smoothedStateVec = smoothedCovMatrix * ( currentWeight * solution+ oldWeight *oldStateVec);

       }
       catch(Exception& u)
       {
             // Throw an exception if something unexpected happens
          ProcessingException e( getClassName() + ":"
                                 + StringUtils::asString( getIndex() ) + ":"
                                 + u.what() );
          GPSTK_THROW(e);
       }

   };

   VariableEstDataMap SolverMWAR::getAmbs(Vector<double>& solution,
                                        Matrix<double>& covMatrix)
   {
       VariableEstDataMap ambEstData;
       for(auto v: currentUnkSet)
       {
           if(v.getArcIndexed())
           {
               ambEstData[v].value = solution(v.getNowIndex());
               ambEstData[v].variance = covMatrix(v.getNowIndex(), v.getNowIndex());
           }
       }
       return ambEstData;
   };

   void SolverMWAR::convertToBLCIndexed(VariableDataMap& fixedVarData,
                                        VariableDataMap& blcIndexedData)
   {

       for(auto ad: fixedVarData)
       {
           Variable blc = ad.first;
           if(blc.getType() == TypeID::BWL12)
           {
               blc.setType(TypeID::BLC12);
               blcIndexedData[blc] = ad.second;
           }
           else if(blc.getType() == TypeID::BWL15)
           {
               blc.setType(TypeID::BLC15);
               blcIndexedData[blc] = ad.second;
           }
           else if(blc.getType() == TypeID::BWL17)
           {
               blc.setType(TypeID::BLC17);
               blcIndexedData[blc] = ad.second;
           }
           else if(blc.getType() == TypeID::BWL18)
           {
               blc.setType(TypeID::BLC18);
               blcIndexedData[blc] = ad.second;
           }
           else if(blc.getType() == TypeID::BWL16)
           {
               blc.setType(TypeID::BLC16);
               blcIndexedData[blc] = ad.second;
           }
           else if(blc.getType() == TypeID::BWL27)
           {
               blc.setType(TypeID::BLC27);
               blcIndexedData[blc] = ad.second;
           }
           else if(blc.getType() == TypeID::BWL26)
           {
               blc.setType(TypeID::BLC26);
               blcIndexedData[blc] = ad.second;
           }
       }
   }

   // print mw values
   void SolverMWAR::printObs( std::ofstream* outStr, 
                            const CommonTime& epoch, 
                            gnssRinex& gData)
   {
       SourceID source = gData.header.source;
       satTypeValueMap stvData = gData.body;
       for(auto stv: stvData)
       {

           double value = stv.second[mwType] ;
           double satArc = stv.second[TypeID::satArc];
           double frac  = value - std::floor(value+0.5);
              // print time
           *outStr << YDSTime(epoch).printf("%Y %03j % 12.6s") << " ";
           *outStr << source << " "
                   << stv.first  << " "
                   << satArc << " "
                   << mwType << " "
                   << rightJustify(asString(value,6),10) << " "
                   << rightJustify(asString(frac,6),10) << " "
                   << endl;
       }
   }

   // print mw values
   void SolverMWAR::printAmbs( std::ofstream* outStr, 
                                  const CommonTime& epoch, 
                                  VariableEstDataMap& ambEstData)
   {
       for(auto amb: ambEstData )
       {
           SourceID source = amb.first.getSource();
           SatID sat       = amb.first.getSatellite();
           TypeID type     = amb.first.getType();
           double satArc   = amb.first.getArc(); 
           double value    = amb.second.value;
           double var      = amb.second.variance;
           double frac     = value - std::floor(value+0.5);

              // print time
           *outStr << YDSTime(epoch).printf("%Y %03j % 12.6s") << " ";
           *outStr << source << " "
                   << sat << " "
                   << rightJustify(asString<int>(int(satArc)),6) << " "
                   << type << " "
                   << rightJustify(asString(value,6),13) << " "
                   << rightJustify(asString(var,6),13) << " "
                   << rightJustify(asString(frac,6),13) << " "
                   << endl;
       }
   }

   // print mw values
   void SolverMWAR::printFixedAmbs( std::ofstream* outStr, 
                                  const CommonTime& epoch, 
                                  VariableEstDataMap& ambEstData,
                                  VariableDataMap& fixedVarData)
   {
       for(auto amb: ambEstData )
       {

           SourceID source = amb.first.getSource();
           SatID sat       = amb.first.getSatellite();
           TypeID type     = amb.first.getType();
           double satArc   = amb.first.getArc(); 
           double value    = amb.second.value;
           double var      = amb.second.variance;
           double frac     = value - std::floor(value+0.5);

           bool fixed(false);    
           double ambFixed(9999);
           if(fixedVarData.find(amb.first)!=fixedVarData.end())
           {
               fixed = true;
               ambFixed = fixedVarData[amb.first];
           }

              // print time
           *outStr << YDSTime(epoch).printf("%Y %03j % 12.6s") << " ";
           *outStr << source << " "
                   << sat << " "
                   << rightJustify(asString<int>(int(satArc)),6) << " "
                   << type << " "
                   << rightJustify(asString(value,6),13) << " "
                   << rightJustify(asString(var,6),13) << " "
                   << rightJustify(asString(frac,6),13) << " "
                   << fixed << " "
                   << ambFixed << " "
                   << endl;
       }
   }


   void SolverMWAR::getRecBias( Vector<double>& solution, 
                                TypeID& recType,
                                double& recBias )
   {
          // Add equation descriptions
       TypeID biasType;
       double biasWL;
       for(auto v: currentUnkSet)
       {
           if(v.getSourceIndexed() && !v.getSatIndexed())
           {
               biasType = v.getType();
               biasWL = solution(v.getNowIndex());
           }
       }

       recType = biasType;
       recBias = biasWL;

   }

   // print mw values
   void SolverMWAR::printFixedInfo( std::ofstream* outStr, 
                                    const CommonTime& epoch, 
                                    TypeID recType,
                                    double recBias, 
                                    int numAmb, 
                                    int numFixed)
   {
          // print time
       *outStr << YDSTime(epoch).printf("%Y %03j % 12.6s") << " "; 
       *outStr << recType << " "; 
       *outStr << recBias << " ";
       *outStr << numAmb  << " ";
       *outStr << numFixed  << " ";
       *outStr << endl;
   }


}  // End of namespace gpstk
